package gwt.seca.client.agents;

import static gwt.g3d.client.math.MatrixStack.MODELVIEW;

import javax.vecmath.Vector3f;
import javax.vecmath.Vector4f;

import ernest72d.Ernest; //Deterministic version
import gwt.seca.client.Block;
import gwt.seca.client.Renderer;
import gwt.seca.client.Resources;
import gwt.seca.client.SceneManager;

public class Ernest7Model extends AbstractModel{
	
	public enum Version {v7_0, v7_1, v7_2}; 
	
	//Animatable parts
	public Vector3f mBodyTrans = new Vector3f();
	public Vector3f mBodyRot = new Vector3f();
	public Vector3f mRightAntennaRot = new Vector3f();
	public Vector3f mLeftAntennaRot = new Vector3f();
	public Vector3f mRightLegRot = new Vector3f();
	public Vector3f mLeftLegRot = new Vector3f();
	
	public Ernest7Model(SceneManager sceneMgr, String name) {
		super(sceneMgr, name);
		mAnimation = new Ernest7Anim(this);
		mDiscrete = true;
	}
	
	/**
	 * Initialize the Ernest agent.
	 */
	public void init() {
		mErnest = new Ernest();
		mErnest.setParameters(6, 1, 6);
		mErnest.addPrimitiveInteraction(">",  50, -80); // Move
		mErnest.addPrimitiveInteraction("^", -20, -50); // Left
		mErnest.addPrimitiveInteraction("v", -20, -50); // Right
		mErnest.addPrimitiveInteraction("-", -10, -10); // Touch
		mErnest.addPrimitiveInteraction("\\",-10, -10); // Touch right
		mErnest.addPrimitiveInteraction("/", -10, -10); // Touch left
		mStatus = false;
	}

	/**
	 * Close the Ernest agent.
	 */
	public void dispose() {
		//TODO
//		mTracer.close();
//		mOrientation = ORIENTATION_UP;
		mErnest = null;
	}
	
	protected void updateAgent(double elaspsedTime) {
		mSchema = stepErnest(mStatus);
		mStatus = enactSchema(mSchema);
	}
	
	protected void drawAgent(Renderer renderer) {
		renderer.enableUniformColor();
		
		//Transformations T Rzyx S
		//Ernest is a ladybird
		//1 body (1 box)
		//	1 head (1 box)
		//		(2 eyes)
		//		2 antennas
		//	6 legs
		{ //Body
			renderer.setColorUniform(new Vector4f(0.8f, 0f, 0f, 1f));
			MODELVIEW.push();
			MODELVIEW.translate(mBodyTrans.x, mBodyTrans.y, mBodyTrans.z-.25f);
			MODELVIEW.rotateZ((float) Math.toRadians(mBodyRot.z));
			MODELVIEW.rotateY((float) Math.toRadians(mBodyRot.y));
			MODELVIEW.rotateX((float) Math.toRadians(mBodyRot.x));
			MODELVIEW.scale(0.3f/2, 0.3f/2, 0.2f/2);
			renderer.setMatrixUniforms();
			Resources.mMeshList[Resources.MESH_ID_BOX].draw();
			{ //Head (1 box)
				renderer.setColorUniform(new Vector4f(0f, 0f, 0f, 1f));
				MODELVIEW.push();
				MODELVIEW.translate(1, 0, 0);
				MODELVIEW.scale(0.5f, 0.5f, 0.5f);
				MODELVIEW.translate(0.5f, 0, 0);
				renderer.setMatrixUniforms();
				Resources.mMeshList[Resources.MESH_ID_BOX].draw();
				{ //Eyes
					
				}
				{ //Antennas
					//Right Antenna
					MODELVIEW.push();
					//First part
					MODELVIEW.translate(1, -0.3f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightAntennaRot.z) + (float) Math.toRadians(-30));
					MODELVIEW.rotateY((float) Math.toRadians(mRightAntennaRot.y) + (float) Math.toRadians(-30));
					MODELVIEW.rotateX((float) Math.toRadians(mRightAntennaRot.x));
					MODELVIEW.scale(1, .1f, .1f);
					MODELVIEW.translate(0.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					//Second part
					MODELVIEW.translate(0.9f, 0, 0);
					MODELVIEW.scale(1, 1/.1f, 1/.1f);
					MODELVIEW.rotateY((float) Math.toRadians(2*30));
					MODELVIEW.scale(1, .1f, .1f);
					MODELVIEW.translate(0.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Left Antenna
					MODELVIEW.push();
					//First part
					MODELVIEW.translate(1, 0.3f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftAntennaRot.z) + (float) Math.toRadians(30));
					MODELVIEW.rotateY((float) Math.toRadians(mLeftAntennaRot.y) + (float) Math.toRadians(-30));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftAntennaRot.x));
					MODELVIEW.scale(1, .1f, .1f);
					MODELVIEW.translate(0.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[1].draw();
					//Second part
					MODELVIEW.translate(0.9f, 0, 0);
					MODELVIEW.scale(1, 1/.1f, 1/.1f);
					MODELVIEW.rotateY((float) Math.toRadians(2*30));
					MODELVIEW.scale(1, .1f, .1f);
					MODELVIEW.translate(0.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[1].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
			}
			//--------------------------------6 legs (1 leg = 1 box)
			{
				renderer.setColorUniform(new Vector4f(0f, 0f, 0f, 1f));
				//Right legs
				MODELVIEW.push();
				MODELVIEW.translate(0, -0.3f, -1);
				{	
					//Middle leg
					MODELVIEW.push();
					MODELVIEW.rotateZ((float) Math.toRadians(-mRightLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Front leg
					MODELVIEW.push();
					MODELVIEW.translate(0.6f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Back leg
					MODELVIEW.push();
					MODELVIEW.translate(-0.6f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
				//Left legs
				MODELVIEW.push();
				MODELVIEW.translate(0, 0.3f, -1);
				{	
					//Middle leg
					MODELVIEW.push();
					MODELVIEW.rotateZ((float) Math.toRadians(-mLeftLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Front leg
					MODELVIEW.push();
					MODELVIEW.translate(.6f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Back leg
					MODELVIEW.push();
					MODELVIEW.translate(-0.6f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
			}
			MODELVIEW.pop();
		}
	}
	
	/**
	 * Run Ernest one step
	 */
	protected String stepErnest(boolean status) {

		// Sense the environment
		//int [][] matrix = new int [2][1];
		
		//m_sensorymotorSystem.senseMatrix(matrix);
		
		String intention = mErnest.step(status);

		return intention;
	}
	
	/**
	 * Enact the primitive schema chosen by Ernest.
	 * @return binary feedback. 
	 */
	protected boolean enactSchema(String schema) {
		
		//TODO Add the tracer
//		System.out.println("Step #" + m_counter + "=======");
//		m_tracer.startNewEvent(m_counter++);

		
		// Sucking the blue square is automatic
		//TODO Add the suck method
		//if (isDirty()) suck();
		
		boolean status = false;
		
	    if (schema.equals("v"))
			status = turnRight();
		else if (schema.equals("^"))
			status = turnLeft();
		else if (schema.equals(">"))
			status = stepForward();
		else if (schema.equals("-"))
			status = touchForward();
		else if (schema.equals("\\"))
			status = touchRight();
		else if (schema.equals("/"))
			status = touchLeft();
		
//		// Play Ernest's internal state
//		if (mErnest.internalState().equals("!"))
//		{
//			//TODO Add sound
////			speak("Oh!", true, false);
////			if (!getSpeakAloud())
////				sleep(200);
//		}
//		else
//			sleep(200); //no need to stop the thread in 3d
		
	    return status;
	}
	
	/**
	 * Turn.
	 * @param angle in degrees. A positive angle will make the agent turn on the left.
	 * @return true if adjacent wall, false if adjacent empty. 
	 */
	protected boolean turn(float angle) {
		mRotation.z += angle;
		return !affordWalk(localToParentRef(DIRECTION_AHEAD));
	}
	protected boolean turnLeft() {
		mAnimation.init(Ernest7Anim.ANIM_TURN_LEFT);
		if (mDiscrete)
			return turn(90);
		else
			return turn(15);
	}
	protected boolean turnRight() {
		mAnimation.init(Ernest7Anim.ANIM_TURN_RIGHT);
		if (mDiscrete)
			return turn(-90);
		else
			return turn(-15);
	}
	
	/**
	 * Move forward.
	 * @return true if adjacent wall, false if adjacent empty. 
	 */
	protected boolean goForward(float distance) {
		Vector3f trans = new Vector3f(DIRECTION_AHEAD);
		trans.scale(distance);
		Vector3f nextLocalFrontPos = new Vector3f(trans);
		//TODO Improve the collision detection
		if (!mDiscrete)
			nextLocalFrontPos.x += getLocalAABB().getHalfSize().x;
		if (affordWalk(localToParentRef(nextLocalFrontPos))) {
			//The adjacent position is a wall
			mAnimation.init(Ernest7Anim.ANIM_BUMP);
			return false;
		} else {
			//The adjacent position is free
			mAnimation.init(Ernest7Anim.ANIM_MOVE);
			mPosition.set(localToParentRef(trans));
			return true;
		}
	}
	protected boolean stepForward() {
		if (mDiscrete)
			return goForward(1);
		else
			return goForward(.1f);
	}
	
	/**
	 * Touch the square forward.
	 * @return true if wall, false if empty. 
	 */
	protected boolean touch(Vector3f vec) {
		Vector3f scenePos = localToParentRef(vec);
		return affordWalk(scenePos);
	}
	protected boolean touchForward() {
		mAnimation.init(Ernest7Anim.ANIM_TOUCH_FORWARD);
		return touch(DIRECTION_AHEAD);
	}
	protected boolean touchLeft() {
		mAnimation.init(Ernest7Anim.ANIM_TOUCH_LEFT);
		return touch(DIRECTION_LEFT);
	}
	protected boolean touchRight() {
		mAnimation.init(Ernest7Anim.ANIM_TOUCH_RIGHT);
		return touch(DIRECTION_RIGHT);
	}
	
	protected boolean affordWalk(Vector3f pos) {
		int i = Math.round(pos.x);
		int j = Math.round(pos.y);
		int k = Math.round(pos.z);
		int blockID = mSceneMgr.getBlockID(i, j, k, mName);
		if (blockID == -1)
			return true;
		return Block.blockList[blockID].getMaterial().affordWalk();
	}
	
	//Agent related
	private Ernest mErnest;
	private String mSchema = "";
	private boolean mStatus = false;
	//Local directions
	final private Vector3f DIRECTION_AHEAD = new Vector3f(1, 0, 0);
	final private Vector3f DIRECTION_LEFT = new Vector3f(0, 1, 0);
	final private Vector3f DIRECTION_RIGHT = new Vector3f(0, -1, 0);
	
}
